#include "main.h"
#include "time.h"
#include "SDL/SDL.h"
#include "Box2D.h"

timespec calcdelta(timespec start, timespec end)
{
    // Calculate no of nano secs that have elapsed
    timespec temp;
    if ((end.tv_nsec-start.tv_nsec)<0)
    {
        temp.tv_sec = end.tv_sec - start.tv_sec - 1;
        // move that 1 second removed above to use here, as 1,000,000,000 ns
        temp.tv_nsec = 1000000000 + end.tv_nsec - start.tv_nsec;
    } else {
        temp.tv_sec = end.tv_sec - start.tv_sec;
        temp.tv_nsec = end.tv_nsec - start.tv_nsec;
    }
    printf("delta: %4.2d, start ns: %4.2d, end ns %4.2d\n", temp.tv_nsec, start.tv_nsec, end.tv_nsec);
    return temp;
}

int main()
{
    timespec timer, newtime;
    Physics *p = new Physics(-100.0f,-100.0f,100.0f,100.0f);
    p->add_walls_to_world(p->boundary_walls);
    b2Vec2 gravity(0.0f, -10.0f);
    p->world->SetGravity(gravity);
    
    // Add a dynamic body just to test
    // Define the dynamic body. We set its position and call the body factory.
	b2BodyDef bodyDef;
	bodyDef.position.Set(0.0f, 4.0f);
	b2Body* body = p->world->CreateBody(&bodyDef);

	// Define another box shape for our dynamic body.
	b2PolygonDef shapeDef;
	shapeDef.SetAsBox(1.0f, 1.0f);

	// Set the box density to be non-zero, so it will be dynamic.
	shapeDef.density = 1.0f;

	// Override the default friction.
	shapeDef.friction = 0.3f;

	// Add the shape to the body.
	body->CreateShape(&shapeDef);

	// Now tell the dynamic body to compute it's mass properties base
	// on its shape.
	body->SetMassFromShapes();
	
    // Test with an additional force
    b2Vec2 impulse(0.1f, 10.0f);
    b2Vec2 offset(0.1f, 0.5f);
    body->ApplyImpulse(impulse, offset);
    
    // timer start
    clock_gettime(CLOCK_REALTIME, &timer);
    
    int wait_useconds = 10000;
    
    // simulate 10 seconds of simulation
    int frames = 10 * 1000000 / wait_useconds;
    
    printf("x    y    angle\n");
    for (int frame=0;frame!=frames;++frame)
    {
        // new time
        clock_gettime(CLOCK_REALTIME, &newtime);
        timespec temp = calcdelta(timer, newtime);
        timer = newtime;
        unsigned long timedelta = (temp.tv_sec*1000000000 + temp.tv_nsec);
        // Update simulation
        p->update(timedelta);
        
        b2Vec2 position = body->GetPosition();
        float32 angle = body->GetAngle();
        printf("%4.2f %4.2f %4.2f    %4.2d\n", position.x, position.y, angle, timedelta);
        
        usleep(wait_useconds);
    }
}
